#ifndef POSITION_KALMANFILTER_H
#define POSITION_KALMANFILTER_H

#include "postprocess/kalman_filter/base_kalmanfilter.h"
// 20230404 这个滤波器暂时不用了
// 这个滤波器用于更新x y z
class PositionKalmanFilter : public BaseKalmanFilter {
 public:
  PositionKalmanFilter();
  PositionKalmanFilter(PositionKalmanFilter const&) = delete;
  PositionKalmanFilter& operator=(PositionKalmanFilter const&) = delete;
  PositionKalmanFilter(PositionKalmanFilter&&) = delete;
  PositionKalmanFilter& operator=(PositionKalmanFilter&&) = delete;
  ~PositionKalmanFilter();

  void Init(int dynam_param_num, int measure_param_num,
            int control_param_num = 0) override;

  /// @brief kalman predict
  /// @param
  /// @return
//  Eigen::VectorXf Predict(double time_diff) override;

  // @brief update filter with object
  /// @param new_object
  /// @param old_object
  /// @param time_diff
  /// @param is_utm_flag
  void UpdateWithObject(TrackedObjectPtr& new_object,
                        TrackedObjectPtr const& old_object,
                        double time_diff) override;

  // @brief set Q
  /// @param Matrix Q
  void SetQ(Eigen::MatrixXf& Q) override;

  // @brief set R
  /// @param Matrix R
  void SetR(Eigen::MatrixXf& R) override;

  /// @brief get state of filter
  /// @param center_point
  /// @param velocity
  /// @param acceleration
  void GetState(Eigen::Vector3f* center_point, float vx, float vy, float ax,
                float ay) noexcept;

  /// @brief update filter without object
  /// @param time_diff
  void UpdateWithoutObject(double time_diff) override;

  /// @brief set initial position
  /// @param velocity to be set
  /// @return void
  void SetCenterPoint(Eigen::Vector3f point) noexcept;

  /// @brief set initial velocity
  /// @param velocity to be set
  /// @return void
  void SetVelocity(Eigen::Vector3f vel) noexcept;

  /// @brief set initial acceleration
  /// @param velocity to be set
  /// @return void
  void SetAcceleration(Eigen::Vector3f acc) noexcept;

 protected:
  /// @brief kalman filter update state with  observation
  /// @param
  void update_state(TrackedObjectPtr const& new_object,
                    TrackedObjectPtr const& old_object,
                    double time_diff) override;

 private:
  /// @brief center point
  Eigen::Vector3f belief_center_point_;
  /// @brief belief velocity
  Eigen::Vector3f belief_velocity_;
  /// @brief belief acceleration
  Eigen::Vector3f belief_acceleration_;
};

#endif
